
#include <sbus.h>
#include <rtthread.h>
#include <control.h>

#define DBG_LEVEL DBG_INFO
#define DBG_SECTION_NAME "com"
#include <rtdbg.h>

static uint8_t *sbus_buffer = RT_NULL;
static uint8_t sbus_ready = 0;
static struct sbus_frame sbus_last_frame;
static rt_sem_t sbus_change_sem = RT_NULL;

int sbus_resovle(sbus_frame_t sbus_frame)
{
    sbus_frame->head = SBUS_FRAME_HEAD;
    sbus_frame->end = SBUS_FRAME_END;
    sbus_frame->flags = sbus_buffer[(SBUS_FRAME_LEN - 2)];

    /* 各个channel */
    sbus_frame->channel[0] = ((int16_t)sbus_buffer[1] >> 0 | ((int16_t)sbus_buffer[2] << 8)) & 0x07FF;
    sbus_frame->channel[1] = ((int16_t)sbus_buffer[2] >> 3 | ((int16_t)sbus_buffer[3] << 5)) & 0x07FF;
    sbus_frame->channel[2] = ((int16_t)sbus_buffer[3] >> 6 | ((int16_t)sbus_buffer[4] << 2) | (int16_t)sbus_buffer[5] << 10) & 0x07FF;
    sbus_frame->channel[3] = ((int16_t)sbus_buffer[5] >> 1 | ((int16_t)sbus_buffer[6] << 7)) & 0x07FF;
    if (sbus_frame->channel[0] == 0 && sbus_frame->channel[1] == 0)
    {
        sbus_ready = 0;
        return -1;
    }
    sbus_frame->channel[4] = ((int16_t)sbus_buffer[6] >> 4 | ((int16_t)sbus_buffer[7] << 4)) & 0x07FF;
    sbus_frame->channel[5] = ((int16_t)sbus_buffer[7] >> 7 | ((int16_t)sbus_buffer[8] << 1) | (int16_t)sbus_buffer[9] << 9) & 0x07FF;
    sbus_frame->channel[6] = ((int16_t)sbus_buffer[9] >> 2 | ((int16_t)sbus_buffer[10] << 6)) & 0x07FF;
    sbus_frame->channel[7] = ((int16_t)sbus_buffer[10] >> 5 | ((int16_t)sbus_buffer[11] << 3)) & 0x07FF;
    sbus_frame->channel[8] = ((int16_t)sbus_buffer[12] << 0 | ((int16_t)sbus_buffer[13] << 8)) & 0x07FF;
    sbus_frame->channel[9] = ((int16_t)sbus_buffer[13] >> 3 | ((int16_t)sbus_buffer[14] << 5)) & 0x07FF;
    sbus_frame->channel[10] = ((int16_t)sbus_buffer[14] >> 6 | ((int16_t)sbus_buffer[15] << 2) | (int16_t)sbus_buffer[16] << 10) & 0x07FF;
    sbus_frame->channel[11] = ((int16_t)sbus_buffer[16] >> 1 | ((int16_t)sbus_buffer[17] << 7)) & 0x07FF;
    sbus_frame->channel[12] = ((int16_t)sbus_buffer[17] >> 4 | ((int16_t)sbus_buffer[18] << 4)) & 0x07FF;
    sbus_frame->channel[13] = ((int16_t)sbus_buffer[18] >> 7 | ((int16_t)sbus_buffer[19] << 1) | (int16_t)sbus_buffer[20] << 9) & 0x07FF;
    sbus_frame->channel[14] = ((int16_t)sbus_buffer[20] >> 2 | ((int16_t)sbus_buffer[21] << 6)) & 0x07FF;
    sbus_frame->channel[15] = ((int16_t)sbus_buffer[21] >> 5 | ((int16_t)sbus_buffer[22] << 3)) & 0x07FF;

    sbus_ready = 0;

    if (sbus_last_frame.channel[5] != sbus_frame->channel[5])
        set_uav_arm_status(sbus_frame->channel[5] == SBUS_VAL_MAX ? arm : lock);

    if (sbus_change_sem == RT_NULL)
        sbus_change_sem = rt_sem_create("sbus_sem", 1, RT_IPC_FLAG_PRIO);
    if (sbus_change_sem)
        rt_sem_take(sbus_change_sem, RT_WAITING_FOREVER);
    sbus_last_frame = *sbus_frame; /* 更新当前的SBUS帧 */
    if (sbus_change_sem)
        rt_sem_release(sbus_change_sem);

    return 0;
}

int sbus_update_buffer(uint8_t *byte_buffer)
{
    if (byte_buffer == RT_NULL)
        return RT_EINVAL;

    sbus_buffer = byte_buffer;
    sbus_ready = 1;
    return 0;
}

inline int sbus_ready_parse()
{
    return sbus_ready == 1 ? 0 : -1;
}

int sbus_print_frame(sbus_frame_t frame)
{
    if (frame == RT_NULL)
        return -1;

    rt_kprintf("head: %02x, end: %02x, flags: %02x\n", frame->head, frame->end, frame->flags);
    for (int i = 0; i < SBUS_CHANNEL_CNT / 2; i++)
    {
        rt_kprintf("%d ", frame->channel[i]);
    }
    rt_kprintf("\n");

    return 0;
}

int sbus_frame_get(sbus_frame_t frame)
{
    if (!frame)
        return -1;

    if (sbus_change_sem)
        rt_sem_take(sbus_change_sem, RT_WAITING_FOREVER);
    *frame = sbus_last_frame;
    if (sbus_change_sem)
        rt_sem_release(sbus_change_sem);

    return 0;
}